#include <iostream>
#include "matrix/math.hpp"
#include "mathlib/math/Limits.hpp"
#include "bodytraced_guidance.h"
#include "px4_interface.h"
#include "guidance_filter/BlosSim.h"
#include "geo/geo.h"

using namespace std;
#define M_PI  3.14159265358979
typedef std::map<int, mavlink_set_position_target_local_ned_t> MAP_LOCAL_WP;

struct Point {
    double x;
    double y;
};

struct DubinsPath {
    // 起点和终点坐标
    Point start_point;
    Point end_point;
    // 起点和终点的速度朝向
    Point startVel;
    Point endVel;
    // 起始圆和终止圆的圆心坐标
    Point start_circle_center;
    Point end_circle_center;
    // 离开起始圆和进入终止圆的切点坐标
    Point start_tangent_point;
    Point end_tangent_point;
    // 判断起始圆上的圆弧是顺时针还是逆时针  顺：true  逆：false
    bool start_clockwise;
    bool end_clockwise;
    // 路径长度
    double path_length;        
};

// 判断圆上一点引出的切向量相对于该圆是顺时针还是逆时针
// 圆心坐标、切点坐标、切向量
bool cal_dir(float x0, float y0, float x_, float y_, float vx, float vy){ 
    // true 顺时针， false 逆时针
    return (y_-y0)*vx - vy*(x_-x0)>=0? true : false;
}

// 计算圆的圆心坐标和切点坐标，并计算路径长度
DubinsPath calculateDubinsPath(Point start, Point end, Point start_vel, Point end_vel, double radius_start, double radius_end) {
    DubinsPath dubins_path; 
    DubinsPath new_dubins_path;
    Point new_start, new_end, new_start_vel, new_end_vel;

    // 计算两个点之间的距离和方向
    double start_theta = atan2(start_vel.y, start_vel.x);
    double end_theta = atan2(end_vel.y, end_vel.x);

    // 起点和终点坐标
    dubins_path.start_point = start;
    dubins_path.end_point   = end;
    // 起始圆的圆心坐标
    dubins_path.start_circle_center.x = start.x - radius_start * sin(start_theta);
    dubins_path.start_circle_center.y = start.y + radius_start * cos(start_theta);
    // 终止圆的圆心坐标
    dubins_path.end_circle_center.x = end.x - radius_end * sin(end_theta);
    dubins_path.end_circle_center.y = end.y + radius_end * cos(end_theta);
    // 计算坐标旋转平移公式中x和y的偏移量，将俩圆中点作为新坐标系下的原点，起始圆在x负半轴，终止圆在x正半轴
    double delta_x = (dubins_path.start_circle_center.x+dubins_path.end_circle_center.x)/2.0; 
    double delta_y = (dubins_path.start_circle_center.y+dubins_path.end_circle_center.y)/2.0; 
    // 计算坐标轴旋转角度
    double delta   = atan2(dubins_path.end_circle_center.y-dubins_path.start_circle_center.y, dubins_path.end_circle_center.x-dubins_path.start_circle_center.x); // 坐标轴旋转角度
    if (delta < 0.0) delta+=2*M_PI;
    double distance = sqrt(pow(dubins_path.start_circle_center.x-dubins_path.end_circle_center.x, 2)+pow(dubins_path.start_circle_center.y-dubins_path.end_circle_center.y, 2));

    // 新坐标系下的起始圆和终止圆的圆心坐标
    new_dubins_path.start_circle_center.x = -distance/2.0;
    new_dubins_path.start_circle_center.y = 0.0;
    new_dubins_path.end_circle_center.x = distance/2.0;
    new_dubins_path.end_circle_center.y = 0.0;
    // 新坐标系下起点和终点的坐标
    new_start.x = (start.x - delta_x)*cos(delta) + (start.y - delta_y)*sin(delta);
    new_start.y = (start.y - delta_y)*cos(delta) - (start.x - delta_x)*sin(delta);
    new_end.x = (end.x - delta_x)*cos(delta) + (end.y - delta_y)*sin(delta);
    new_end.y = (end.y - delta_y)*cos(delta) - (end.x - delta_x)*sin(delta);
    // 新坐标系下初始速度矢量和终止速度矢量的方向

    new_start_vel.x = start_vel.x*cos(delta) + start_vel.y*sin(delta);
    new_start_vel.y = start_vel.y*cos(delta) - start_vel.x*sin(delta);
    new_end_vel.x   = end_vel.x*cos(delta) + end_vel.y*sin(delta);
    new_end_vel.y   = end_vel.y*cos(delta) - end_vel.x*sin(delta);

    // 判断两向量在圆上是顺时针(_clockwise = true)还是逆时针(逆时针 _clockwise = false) 
    dubins_path.start_clockwise = cal_dir(new_dubins_path.start_circle_center.x, new_dubins_path.start_circle_center.y, new_start.x, new_start.y, new_start_vel.x, new_start_vel.y);
    dubins_path.end_clockwise   = cal_dir(new_dubins_path.end_circle_center.x, new_dubins_path.end_circle_center.y, new_end.x, new_end.y, new_end_vel.x, new_end_vel.y);

    // 初始化新坐标系下两圆上的切点坐标
    Point new_start_tangency, new_end_tangency;

    // 新坐标系下俩圆共4条切线，根据同顺同逆原则必是4条切线中的一条
    if(dubins_path.start_clockwise && dubins_path.end_clockwise){ // 起始圆、终止圆均为顺时针，对应上方的外切线
        // 求两个圆上的外切点
        new_start_tangency.x = new_dubins_path.start_circle_center.x;
        new_start_tangency.y = abs(radius_start);
        new_end_tangency.x   = new_dubins_path.end_circle_center.x;
        new_end_tangency.y   = abs(radius_end);

        // 计算新坐标系下 起始点相对于新x轴正方向的夹角，保持在[0,2*PI]内
        double angle1 = atan2(new_start_vel.y, new_start_vel.x);
        if (angle1 < 0.0) angle1+=2*M_PI;
        double arc1 = angle1 - 0; // 起始圆上方外切线的速度朝向与x轴正向夹角为0
        double arc1_length = arc1 * abs(radius_start); // 计算第一段圆弧长

        double distance = sqrt(pow(new_start_tangency.x-new_end_tangency.x, 2)+pow(new_start_tangency.y-new_end_tangency.y, 2));

        double angle2 = atan2(new_end_vel.y, new_end_vel.x);
        if (angle2 < 0.0) angle2+=2*M_PI;
        double arc2 = 2*M_PI - (angle2-0); // 终止圆的上方外切线朝向是x轴正向，所以此处也是减0
        double arc2_length = arc2 * abs(radius_end); // 计算第二段圆弧长
        dubins_path.path_length = arc1_length + distance + arc2_length;  // 计算总路径长
    }else if (dubins_path.start_clockwise && !dubins_path.end_clockwise){ // 起始圆顺、终止圆逆，对应左上到右下的内切线
        // 求两个圆上的内切点
        double sin_alpha = abs(radius_start/new_dubins_path.start_circle_center.x); // sin_alpha：内切线与x轴夹角的sin值
        new_start_tangency.x = -abs(abs(new_dubins_path.start_circle_center.x)-abs(radius_start)*sin_alpha);
        new_start_tangency.y = abs(radius_start*sqrt(1-pow(sin_alpha,2)));
        new_end_tangency.x   = abs(abs(new_dubins_path.end_circle_center.x)-abs(radius_end)*sin_alpha);
        new_end_tangency.y   = -abs(radius_end*sqrt(1-pow(sin_alpha,2)));

        double start_arc_tangency = atan2(new_end_tangency.y-new_start_tangency.y, new_end_tangency.x-new_start_tangency.x); 
        if (start_arc_tangency < 0.0) start_arc_tangency+=2*M_PI;
        double angle1 = atan2(new_start_vel.y, new_start_vel.x);
        if (angle1 < 0.0) angle1+=2*M_PI;
        double arc1 = angle1 >= start_arc_tangency ? angle1 - start_arc_tangency: 2*M_PI - (start_arc_tangency - angle1);
        double arc1_length = arc1 * abs(radius_start); // 计算第一段圆弧长

        double distance = sqrt(pow(new_start_tangency.x-new_end_tangency.x, 2)+pow(new_start_tangency.y-new_end_tangency.y, 2));

        double end_arc_tangency = start_arc_tangency;
        double angle2 = atan2(new_end_vel.y, new_end_vel.x);
        if (angle2 < 0.0) angle2+=2*M_PI;
        double arc2 = angle2 >= end_arc_tangency ? angle2 - end_arc_tangency: 2*M_PI - (end_arc_tangency - angle2);
        double arc2_length = arc2 * abs(radius_end); // 计算第二段圆弧长
        dubins_path.path_length = arc1_length + distance + arc2_length;  // 计算总路径长
    }else if (!dubins_path.start_clockwise && dubins_path.end_clockwise){ // 起始圆逆、终止圆顺，对应左下到右上的内切线
        // 求两个圆上的内切点
        double sin_alpha = abs(radius_start/new_dubins_path.start_circle_center.x);
        new_start_tangency.x = -abs(abs(new_dubins_path.start_circle_center.x)-abs(radius_start)*sin_alpha);
        new_start_tangency.y = -abs(radius_start*sqrt(1-pow(sin_alpha,2)));
        new_end_tangency.x   = abs(abs(new_dubins_path.end_circle_center.x)-abs(radius_end)*sin_alpha);
        new_end_tangency.y   = abs(radius_end*sqrt(1-pow(sin_alpha,2)));
        
        double start_arc_tangency = atan2(new_end_tangency.y-new_start_tangency.y, new_end_tangency.x-new_start_tangency.x); 
        if (start_arc_tangency < 0.0) start_arc_tangency+=2*M_PI;
        double angle1 = atan2(new_start_vel.y, new_start_vel.x);
        if (angle1 < 0.0) angle1+=2*M_PI;
        double arc1 = angle1 <= start_arc_tangency ? start_arc_tangency - angle1 : 2*M_PI - (angle1 - start_arc_tangency);
        double arc1_length = arc1 * abs(radius_start); // 计算第一段圆弧长

        double distance = sqrt(pow(new_start_tangency.x-new_end_tangency.x, 2)+pow(new_start_tangency.y-new_end_tangency.y, 2));

        double end_arc_tangency = start_arc_tangency;
        double angle2 = atan2(new_end_vel.y, new_end_vel.x);
        if (angle2 < 0.0) angle2+=2*M_PI;
        double arc2 = angle2 <= end_arc_tangency ? end_arc_tangency - angle2 : 2*M_PI - (angle2 - end_arc_tangency);
        double arc2_length = arc2 * abs(radius_end); // 计算第二段圆弧长
        dubins_path.path_length = arc1_length + distance + arc2_length;  // 计算总路径长
    }else{ // 起始圆、终止圆均为逆时针，对应下方的外切线
        // 求两个圆上的外切点
        new_start_tangency.x = new_dubins_path.start_circle_center.x;
        new_start_tangency.y = -abs(radius_start);
        new_end_tangency.x   = new_dubins_path.end_circle_center.x;
        new_end_tangency.y   = -abs(radius_end);

        double angle1 = atan2(new_start_vel.y, new_start_vel.x);
        if (angle1 < 0.0) angle1+=2*M_PI;
        double arc1 = 2*M_PI - (angle1-0); // 起始圆的上方外切线朝向是x轴正向，所以此处减0
        double arc1_length = arc1 * abs(radius_start); // 计算第一段圆弧长
        double distance = sqrt(pow(new_start_tangency.x-new_end_tangency.x, 2)+pow(new_start_tangency.y-new_end_tangency.y, 2));
        double angle2 = atan2(new_end_vel.y, new_end_vel.x);
        if (angle2 < 0.0) angle2+=2*M_PI;
        double arc2 = angle2-0; // 终止圆的上方外切线朝向是x轴正向，所以此处也是减0
        double arc2_length = arc2 * abs(radius_end); // 计算第二段圆弧长
        dubins_path.path_length = arc1_length + distance + arc2_length;  // 计算总路径长
    }
    
    dubins_path.start_tangent_point.x = new_start_tangency.x*cos(delta) - new_start_tangency.y*sin(delta)+delta_x;
    dubins_path.start_tangent_point.y = new_start_tangency.y*cos(delta) + new_start_tangency.x*sin(delta)+delta_y;
    dubins_path.end_tangent_point.x   = new_end_tangency.x*cos(delta) - new_end_tangency.y*sin(delta)+delta_x;
    dubins_path.end_tangent_point.y   = new_end_tangency.y*cos(delta) + new_end_tangency.x*sin(delta)+delta_y;
    dubins_path.startVel = start_vel;
    dubins_path.endVel = end_vel;
    return dubins_path;
}

// 航点管理
struct wayPointManager{
    double lat;
    double lon;
    float alt;
    float ned_x;
    float ned_y;
    float ned_z;
    uint16_t px4_type_mask;
}WP_takeoff, WP_target;

// Dubins曲线类型
enum dubinsCurveType{
    circle = 0,
    line
};

// 飞行任务阶段
enum flightStage{
    takeoffMode=0,
    cruiseMode,
    guidanceMode,
    landMode,
    testMode
};

void generateMavlinkWP(MAP_LOCAL_WP &local_wp_map, const vector<pair<float, float>> wpList)
{
    /*将航点信息转换为mavlink格式*/
    local_wp_map.clear();
    mavlink_set_position_target_local_ned_t local_wp{}; //航点初始化模板
    local_wp.target_component = MAV_COMP_ID_AUTOPILOT1;
    local_wp.type_mask = 0b0000111111000000; //指定掩码
    local_wp.z = -30;
    local_wp.vx = 30;
    local_wp.coordinate_frame = MAV_FRAME_LOCAL_NED;
    for (ssize_t i=0; i<wpList.size(); i++)
    {
        local_wp_map[i] = local_wp;
        local_wp_map[i].x = wpList[i].first;
        local_wp_map[i].y = wpList[i].second;
    }
}

Point AdjDist(const int length, const Point point, const Point centerPoint, const bool clockwWise, const float bonusDeg = 0.f)
{
    /*计算切线方向上的回正距离，即引导飞行器沿期望路径飞行的给定引导航点与期望到达航点之间的欧式距离*/
    Point AdjDist{};
    float bonusRad = bonusDeg * M_PI / 180;
    if (!clockwWise){   //此时由于坐标系变化，需改变顺逆试针方向标志的正负
        AdjDist.x = length * sin(atan2((point.x - centerPoint.x), (point.y - centerPoint.y)) - M_PI / 2 - bonusRad); //此时适用于顺时针
        AdjDist.y = length * cos(atan2((point.x - centerPoint.x), (point.y - centerPoint.y)) - M_PI / 2 - bonusRad);
    }
    else{
        AdjDist.x = length * sin(atan2((point.x - centerPoint.x), (point.y - centerPoint.y)) + M_PI / 2 + bonusRad); //此时适用于逆时针
        AdjDist.y = length * cos(atan2((point.x - centerPoint.x), (point.y - centerPoint.y)) + M_PI / 2 + bonusRad);
    }
    return AdjDist;
}

void generateWayPoint(MAP_LOCAL_WP &local_wp_map, const DubinsPath path)
{
    /*利用预设信息生成航点序列*/
    vector<pair<float, float>> wpList;
    Point adjDist{};   //回正距离,当前圆弧上参考点

    //起始过渡阶段提前确保速度矢量回正，需预留足够距离
    adjDist = AdjDist(100, path.start_point, path.start_circle_center, path.start_clockwise);
    wpList.push_back(make_pair(path.start_point.x - adjDist.x, path.start_point.y - adjDist.y));
    wpList.push_back(make_pair(path.start_point.x, path.start_point.y));
    wpList.push_back(make_pair(path.start_point.x + adjDist.x, path.start_point.y + adjDist.y));

    //起始圆绕圈
    for (int i=0; i< int(abs(atan2(path.start_point.x, path.start_point.y) - atan2(path.start_tangent_point.x, path.start_tangent_point.y)) / M_PI * 180 / 15);i++)
    {
        adjDist = AdjDist(100, path.start_point, path.start_circle_center, path.start_clockwise, i * 30); // 距离当前圆弧段参考点100*sqrt(2)m且偏离其切线方向45°处的回调航点信息
        wpList.push_back(make_pair(path.start_circle_center.x + adjDist.x, path.start_circle_center.y + adjDist.y));
    }

    //起始圆终止阶段
    adjDist = AdjDist(100, path.start_tangent_point, path.start_circle_center, path.start_clockwise);
    wpList.push_back(make_pair(path.start_tangent_point.x, path.start_tangent_point.y));
    wpList.push_back(make_pair(path.start_tangent_point.x + adjDist.x, path.start_tangent_point.y + adjDist.y));    // 回正处理，保证路线平滑

    //直线段与终止圆过渡段
    adjDist = AdjDist(100, path.end_tangent_point, path.end_circle_center, path.end_clockwise);
    wpList.push_back(make_pair(path.end_tangent_point.x, path.end_tangent_point.y));
    wpList.push_back(make_pair(path.end_tangent_point.x + adjDist.x, path.end_tangent_point.y + adjDist.y));    // 回正处理，保证路线平滑

    //终止圆绕圈
    for (int i=0; i< int(abs(atan2(path.end_point.x, path.end_point.y) - atan2(path.end_tangent_point.x, path.end_tangent_point.y)) / M_PI * 180 / 15);i++)
    {
        adjDist = AdjDist(100, path.end_tangent_point, path.end_circle_center, path.end_clockwise, i * 30); // 距离当前圆弧段参考点100*sqrt(2)m且偏离其切线方向45°处的回调航点信息
        wpList.push_back(make_pair(path.end_circle_center.x + adjDist.x, path.end_circle_center.y + adjDist.y));
    }

    //起始圆终止阶段
    adjDist = AdjDist(100, path.end_point, path.end_circle_center, path.end_clockwise);
    wpList.push_back(make_pair(path.end_point.x, path.end_point.y));
    wpList.push_back(make_pair(path.end_point.x + adjDist.x, path.end_point.y + adjDist.y));
    wpList.push_back(make_pair(path.end_point.x + 5 * adjDist.x, path.end_point.y + 5 * adjDist.y));    //终点处绕圈盘旋

    generateMavlinkWP(local_wp_map, wpList);
}

ssize_t getLength(const MAP_LOCAL_WP &local_wp_map){
/*获取航点序列长度，即航点个数*/
    ssize_t listLength = local_wp_map.size();
    if (listLength > 0) {
        return listLength;
    } else {
        std::cout << "[formation] Got wrong local wp list " << std::endl;
        return -1;
    }
}

int main() {
    printf("build_branch:\t%s\nbuild_sha1:\t%s\nbuild_time:\t%s %s\n",
           GIT_BRANCH_NAME, GIT_COMMIT_SHA1, __DATE__, __TIME__);
           
    // 获取飞机状态
    Px4Interface px4_interface(26540, 24580, "127.0.0.1");
    UavStatus px4_info;

    /*以下是offboard 控制变量初始化;*/
    mavlink_message_t msg;

    //航点信息初始化
    ssize_t curWpSeq {0};   // 当前航点索引
    ssize_t wp_count{10};   // 航点总数
    float dist_xy, dist_z;  //本机与终点距离
    MAP_LOCAL_WP local_wp_map{};    //存储给定航点信息
    mavlink_local_position_ned_t px4_local_pos; //本机实时local position

    //飞行模式
    uint8_t curFlightStage{takeoffMode};
    ssize_t numLoop{1}; //巡航任务重复次数，置0时为不重复
    uint8_t finalStage{guidanceMode};

    /*Dubins轨迹变量初始化与计算*/
    Point start_point, end_point;
    Point start_vel, end_vel;
    double radius;

    start_point.x = 800.0;
    start_point.y = 0.0;
    end_point.x = 400.0;
    end_point.y = 800.0;
    start_vel.x = 30.0;
    start_vel.y = 0.0;
    end_vel.x = -30.0;
    end_vel.y = -40.0;
    radius = 100.0;

    std::vector<DubinsPath> dubins_paths;

    // 计算所有Dubins路径
    dubins_paths.push_back(calculateDubinsPath(start_point, end_point, start_vel, end_vel, radius, radius));
    dubins_paths.push_back(calculateDubinsPath(start_point, end_point, start_vel, end_vel, radius, -radius));
    dubins_paths.push_back(calculateDubinsPath(start_point, end_point, start_vel, end_vel, -radius, radius));
    dubins_paths.push_back(calculateDubinsPath(start_point, end_point, start_vel, end_vel, -radius, -radius));

    // 计算最短路径
    double shortest_length = INT_MAX;
    DubinsPath shortest_path;

    for (const auto& path : dubins_paths) {
        if (path.path_length < shortest_length) {
            shortest_length = path.path_length;
            shortest_path = path;
        }
    }

    // 打印结果
    std::cout << "Shortest Dubins Path:" << std::endl;
    std::cout << "起点坐标: (" << shortest_path.start_point.x << ", " << shortest_path.start_point.y << ")" << std::endl;
    std::cout << "起始圆的圆心坐标: (" << shortest_path.start_circle_center.x << ", " << shortest_path.start_circle_center.y << ")" << std::endl;
    std::cout << "起始圆的切点坐标: (" << shortest_path.start_tangent_point.x << ", " << shortest_path.start_tangent_point.y << ")" << std::endl;

    std::cout << "终点坐标: (" << shortest_path.end_point.x << ", " << shortest_path.end_point.y << ")" << std::endl;
    std::cout << "终止圆的圆心坐标: (" << shortest_path.end_circle_center.x << ", " << shortest_path.end_circle_center.y << ")" << std::endl;
    std::cout << "终止圆切点: (" << shortest_path.end_tangent_point.x << ", " << shortest_path.end_tangent_point.y << ")" << std::endl;
    std::cout << "总路径长度: " << shortest_path.path_length << " m" <<std::endl;

    // 时延，保证初始化完成
    usleep(3 * 1000 * 1000);
    while(true) {
        // 判断是否获得飞控信息
        if (px4_interface.updated(px4_info) && px4_info.home_pos.time_usec > 0) {

            //判断本机飞行模式，根据情况解锁
            uint8_t mainMode = (px4_info.heartbeat.custom_mode >> 16) & 0xff;
            uint8_t baseMode = px4_info.heartbeat.base_mode;
            cout<<"mainMode:"<<(int)mainMode<<endl;
            cout<<"baseMode:"<<(int)baseMode<<endl;
            cout<<"systemStatus:"<<(int)px4_info.heartbeat.system_status<<endl;

            if (!(px4_info.heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
            // 解锁
                px4_interface.send_arm_to_px4();
            }

            // local 坐标系初始化，以指定点为原点，默认为起飞阶段起始点
            globallocalconverter_init(px4_info.home_pos.latitude * 1e-7,
                                    px4_info.home_pos.longitude * 1e-7,
                                    px4_info.home_pos.altitude * 1e-3,
                                    currentTimeUs());
            cout<<"px4_home_pos:"<< px4_info.home_pos.latitude <<","<< px4_info.home_pos.longitude <<","<< px4_info.home_pos.altitude <<endl;
            // 获得飞控的实时local坐标点
            globallocalconverter_tolocal(px4_info.globalPos.lat * 1e-7,
                                        px4_info.globalPos.lon * 1e-7,
                                        px4_info.globalPos.alt * 1e-3f,
                                        &px4_local_pos.x, &px4_local_pos.y, &px4_local_pos.z);

            cout<<"px4_global_pos:"<< px4_info.globalPos.lat <<","<< px4_info.globalPos.lon <<","<< px4_info.globalPos.alt <<endl;
            cout<<"px4_local_pos:"<< px4_local_pos.x <<","<< px4_local_pos.y <<","<< px4_local_pos.z <<endl;
            // 阶段性发航点指令
            if (curFlightStage==takeoffMode)
            {
                WP_target.ned_x = 400;
                WP_target.ned_y = 0;
                WP_target.ned_z = -30;
            }
            else if (curFlightStage==cruiseMode)
            {
                // 生成航点序列
                generateWayPoint(local_wp_map, shortest_path); 
                //获取航点序列长度
                wp_count = getLength(local_wp_map); 
                WP_target.ned_x = local_wp_map[curWpSeq].x;
                WP_target.ned_y = local_wp_map[curWpSeq].y;
                WP_target.ned_z = local_wp_map[curWpSeq].z;
            }

            mavlink_wpm_distance_to_point_local(px4_local_pos.x, px4_local_pos.y, px4_local_pos.z,
            WP_target.ned_x, WP_target.ned_y, WP_target.ned_z, &dist_xy, &dist_z);

            cout<<"dist_xy:"<<dist_xy<<endl;

            if (dist_xy < 100) {    
                if (curWpSeq < wp_count -1){
                    if (curFlightStage == cruiseMode) ++curWpSeq;
                    else if (curFlightStage == takeoffMode) ++curFlightStage;
                    cout<< "Vehicle reaches target, change to next" << curWpSeq << endl;
                    WP_target.ned_x = local_wp_map[curWpSeq].x;
                    WP_target.ned_y = local_wp_map[curWpSeq].y;
                    WP_target.ned_z = local_wp_map[curWpSeq].z;
                    cout<< "x_target"<<(float)WP_target.ned_x<<", y_target"<<(float)WP_target.ned_y<<endl;
                } else {
                    if (curFlightStage == cruiseMode){
                        cout<<"mission complete, back to the first wp"<<endl;
                    }
                }
            }
            cout<<"curWpSeq:"<<curWpSeq<<endl;
            cout<<"curFlightMode:"<<(int)curFlightStage<<endl;
            cout<<"target_pos:"<< WP_target.ned_x <<","<< WP_target.ned_y <<","<< WP_target.ned_z <<endl;

            // 发送航点
            if (curFlightStage == takeoffMode)
            px4_interface.send_takeoff_to_px4(WP_target.ned_x, WP_target.ned_y, WP_target.ned_z);
            else 
            px4_interface.send_local_pos_target_to_px4(WP_target.ned_x, WP_target.ned_y, WP_target.ned_z);

        } else {
            // 没有获取到飞控信息，等待
            usleep(50 * 1000);
        }
    }
    return 0;
}
